#include "mavlinkdataprocessor.h"
#include "log.h"

MavlinkPayloadManager<mavlink_attitude_t> mavlinkManagerAttitude;

void MavlinkDataProcessor::processData()
{
    LOGI("MavlinkDataProcessor thread ID:0x%x", QThread::currentThreadId());

    int size = 0;
    int cycles = 0;
    QString string;

    mavlink_status_t status;
    mavlink_message_t msg;
    mavlink_attitude_t *attitude;
    int chan = MAVLINK_COMM_0;
    while(!stop)
    {
        string.clear();
        mutexListUdpReceivedData.lock();
//        qDebug() << "list size:" << listUdpReceivedData.size();

        // wait for data
        while(!listUdpReceivedData.size() && !stop){
            mutexListUdpReceivedData.unlock();
//            qDebug() << "no data,sleep 100ms now";
            QThread::msleep(100);
            mutexListUdpReceivedData.lock();
        }

        while (listUdpReceivedData.size())
        {
            // get first data package from list
            QByteArray * array = listUdpReceivedData.front();
            // remove it from list
            listUdpReceivedData.removeFirst();
            // release the list, so UDP data receiver can use it
            mutexListUdpReceivedData.unlock();

            // process the data package now
            size += array->size();
            string += QString::number(array->size());
            string += "bytes: ";
            string.append(array->toHex(' ').toUpper());
            string.append("\n");

            for(int i = 0; i < array->size(); i++)
            {
                if (mavlink_parse_char(chan, array->at(i), &msg, &status))
                {
//                    qDebug("Received message with ID %d, sequence: %d from component %d of system %d\n",
//                           msg.msgid, msg.seq, msg.compid, msg.sysid);
                    // ... DECODE THE MESSAGE PAYLOAD HERE ...
                    switch(msg.msgid) {
                    case MAVLINK_MSG_ID_ATTITUDE:// 30
                    {
                        attitude = new mavlink_attitude_t;
                        mavlink_msg_attitude_decode(&msg, attitude);
                        mavlinkManagerAttitude.add(attitude);
//                        qDebug("pitch:%f roll:%f yaw:%f\n",
//                               attitude.pitch, attitude.roll, attitude.yaw);
                        emit attitudeReady(&attitude);
                    }
                        break;
                    }
                    emit messageReady(msg);
                }
            }

            // release memory
            delete array;

            // lock for next loop
            mutexListUdpReceivedData.lock();
        }
        mutexListUdpReceivedData.unlock();
        // update text for UDP raw data in HEX format
        emit stringReady(string);
        cycles++;
        if (cycles % 100 == 0)
            LOGD("processed %d bytes data in thread:", size);
        QThread::msleep(100);
    }
}
